#pragma once

//#include "Common.h"

#include <vbl/util/Array2D.h>

#include <vbl/img/Image.h>
#include <vbl/img/ImageAdaptorImage.h>
#include <vbl/img/proc/ImageUndistort.h>
#include <vbl/calib/BogUtils.h>
#include <vbl/math/Utilities.h>

// DOUWE: ADDED
#include <vbl/util/FileUtils.h>
#include <vbl/util/StringUtils.h>
#include <vbl/img/ImageDefs.h>
//

using namespace std;
using namespace vbl;

namespace cas
{
namespace help
{

/// Euclidean distance struct used for passing the euclidean distance function as a parameter
struct euclideanDistance
{
	double operator()( const math::Vector2& l, const math::Vector2& r )
	{
		return math::euclideanDistanceSq<2, double>( l, r );
	}
};

/// Compute sum of log values
/// @param[in]	val1
/// @param[in]	val2
/// @return		Sum of log values val1 and val2
double computeLogSum( 
	const double val1,
	const double val2 
);

/// read camera calibration and compute camera matrices
/// @param[in]	bogDir
/// @param[out]	bogData
void readBogData(
	const string& bogDir,
	calib::BogData* bogData
	)
{
	for ( size_t i = 0; ; ++i )
	{
		const string bogFilename( bogDir + "calib_k" + util::nrToString( i, 1 ) + ".bog" );
		if ( !util::fileExists( bogFilename ) )
			break;
		bogData[i] = calib::readBogFile( bogFilename );		
	}
}

/// Compute undistortion mapping for a specific camera
/// @param[in]	bogData
/// @param[out]	mapXMat
/// @param[out]	mapYMat
void computeUndistortMap(
	const calib::BogData& bogData,
	util::Array2D< double >& mapXMat,
	util::Array2D< double >& mapYMat
	)
{
	double fx = bogData.intr.fc_u;
	double fy = bogData.intr.fc_v;
	double cx = bogData.intr.cc_u;
	double cy = bogData.intr.cc_v;
	double k1 = bogData.intr.kc_1;
	double k2 = bogData.intr.kc_2;
	double p1 = bogData.intr.kc_3;
	double p2 = bogData.intr.kc_4;
	img::proc::makeUndistortMap( mapXMat, mapYMat, fx, fy, cx, cy, k1, k2, p1, p2, true );	
}

/// Undistort image given pre-computed undistortion maps
/// @param[in]	capturedImage
/// @param[in]	mapXMat
/// @param[in]	mapYMat
template <typename T, template <class> class AP1>
void undistortImage(
	img::Image< T, AP1 >& capuredImage,
	const util::Array2D< double >& mapXMat,
	const util::Array2D< double >& mapYMat
	)
{
	img::Image<T, AP1> undistortedImage;	
	undistortImage( capuredImage, undistortedImage, mapXMat, mapYMat );
	capuredImage = undistortedImage;
}

/// Undistort image given pre-computed undistortion maps
/// @param[in]	capturedImage
/// @param[in]	undistortedImage
/// @param[in]	mapXMat
/// @param[in]	mapYMat
template <typename T, template <class> class AP1, template <class> class AP2>
void undistortImage(
	const img::Image< T, AP1 >& capuredImage,
	img::Image< T, AP2 >& undistortedImage,
	const util::Array2D< double >& mapXMat,
	const util::Array2D< double >& mapYMat
	)
{
	undistortedImage.resize( capuredImage.size() );
	
	// we need to adapt the access policy of the input image for the
	// undistort() method as the interpolation may try to sample outside the image boundries
	typedef img::Image< T, img::BorderedAccess > BorderedAccessImage;
	BorderedAccessImage imProxy = img::wrapImage< img::BorderedAccess >( capuredImage );
	BorderedAccessImage imProxy2 = img::wrapImage< img::BorderedAccess >( undistortedImage );
	img::proc::undistort< img::InterpolatorBilinear >( imProxy, imProxy2, mapXMat, mapYMat );
}

// DOUWE: Commented
/// Project image onto ground plane
/// @param[in]	image
/// @param[in]	camera
/// @param[out]	projIm
/// @param[in]	zLevel
//void projectTopDown(
//	const img::ImageRGB& image,
//	const int camera,
//	ProjectedImages& projIm,	
//	const int zLevel = 0
//);

/// Reconstruc ID image for a specific object set using the scene's depth map and set id list
/// @param[in]	depthIms
/// @param[in]	combinationMask
/// @param[in]	combinationIdList
/// @param[in]	cam
/// @param[out]	setImage
template <typename T>
void getObjectSetImFromDepthMap( 
	const vector< vector< img::Image< T > > >& depthIms,
	const img::ImageG& combinationMask,
	const vector< size_t >& combinationIdList,
	const size_t cam,
	img::ImageG& setImage 
	)
{
	setImage.resize( combinationMask.size(), (unsigned char)0 );

	/// Go over teh id image and fill the pixels with the correct person ID
	for( int y = 0; y < setImage.height(); ++y )
	{
		for( int x = 0; x < setImage.width(); ++x )		
		{
			if( combinationMask( x, y ) > (unsigned char)0 )
			{
				T closestPersonDist = numeric_limits<T>::max();
				size_t closestPersonId = 0;

				/// Determine which of the people is closest to the camera
				BOOST_FOREACH( const size_t& personId, combinationIdList )
				{
					size_t personIdOffset = personId + 1;
					if ( depthIms[cam][personIdOffset]( x, y ) > 0 && 
						 depthIms[cam][personIdOffset]( x, y ) < closestPersonDist )
					{
						closestPersonDist = depthIms[cam][personIdOffset]( x, y );
						closestPersonId = personIdOffset;
					}
				}

				setImage( x, y ) = (unsigned char)closestPersonId;
			}
		}
	}
}

struct errorEllipse
{
	double xVar;
	double yVar;
	double xAngle;
	double yAngle;
};

/// Compute an error ellipse based on a mean and covariance
/// @param[in]  cov
///	@param[out] errEl
void computeErrorEllipse(
	const math::Matrix2& cov,
	errorEllipse& errEl
);

// DOUWE: Commented
/// Draw an error ellipse based on a mean and covariance
/// @param[in]  mean
/// @param[in]  covariance
/// @param[in]  color
/// @param[in]  thickness
/// @param[in]  sigma
/// @param[out] image
///	@param[out] errEl
//void drawErrorEllipse(
//	const math::Vector2& mean,
//	const math::Matrix2& cov,
//	const img::Triple<>& color,
//	const int thickness,
//	const double sigma,	
//	ProjectedImages& image,
//	errorEllipse& errEl = errorEllipse()
//);

} // namespace help
} // namespace cas